#include <iostream>
#include <thread>
#include <atomic>

#include "ros/ros.h"
#include "actionlib/server/action_server.h"
#include "demo/MoveAction.h"

std::atomic<bool> g_isCanceled{false};

void done_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> done_cb");
    if (goalHandle.getGoal()->destination > 100) {
        goalHandle.setRejected(); // 拒绝请求
        return;
    }
    goalHandle.setAccepted(); // 同意请求
    ros::Rate rate(1);
    uint32_t distance = 0;
    while (ros::ok() && distance < goalHandle.getGoal()->destination) {
        if (g_isCanceled) {
            goalHandle.setCanceled(); // 取消任务
            return;
        }
        demo::MoveFeedback feedback;
        feedback.distance = distance;
        goalHandle.publishFeedback(feedback);
        ROS_INFO("==> distance: %d", distance);
        rate.sleep();
        ++distance;
    }
    demo::MoveResult result;
    result.result = distance;
    goalHandle.setSucceeded(result); // 执行成功
}

void goal_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> goal_cb, goal: %d", goalHandle.getGoal()->destination);
    g_isCanceled = false;
    std::thread t(done_cb, goalHandle);
    t.detach();
}

void cancel_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> cancel_cb");
    g_isCanceled = true;
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "server");
    ros::NodeHandle nodeHandle;
    std::string actionName = "demo6";
    actionlib::ActionServer<demo::MoveAction> server(
        nodeHandle,
        actionName,
        boost::bind(goal_cb, _1),
        boost::bind(cancel_cb, _1),
        false
    );
    server.start();
    ros::spin();

    return EXIT_SUCCESS;
}